#include "sv2_aruco_det.h"
#include <cmath>
#include <opencv2/calib3d.hpp>
#include <opencv2/dnn.hpp>
#include <fstream>


namespace sv2 {

using namespace cv;
using namespace cv::dnn;


void ArucoDetCppNode::getIdsWithLengths(std::vector<int>& ids_, std::vector<double>& lengths_)
{
  ids_ = this->_ids_need;
  lengths_ = this->_lengths_need;
}


void ArucoDetCppNode::run()
{
  while (this->is_running())
  {
    // 主线程阻塞，等待队里中出现数据
    nlohmann::json json_image;
    cv::Mat image;
    if (this->_use_own_camera == 1)
    {
      std::unique_lock<std::mutex> lock(this->_camera_queue_mtx);
      this->_camera_cv.wait(lock, [this] { return !this->_camera_queue.empty(); });

      image = this->_camera_queue.front();
    }
    else
    {
      std::unique_lock<std::mutex> lock(this->_image_queue_mtx);
      this->_image_cv.wait(lock, [this] { return !this->_image_queue.empty(); });

      json_image = this->_image_queue.front();
      this->_image_queue.pop();

      image = sms::sms2cvimg(json_image);
    }
    nlohmann::json res_msg = this->detect(image);

    if (this->_use_own_camera == 1)
    {
      if (res_msg.contains("fei_cxcy"))
      {
        int t_cx = res_msg["fei_cxcy"][0];
        int t_cy = res_msg["fei_cxcy"][1];
        cv::circle(image, cv::Point(t_cx, t_cy), 8, cv::Scalar(154, 250, 0), 2);
      }
      cv::imshow("image", image);
      cv::waitKey(5);
    }
    else
    {
      json_image["spirecv_msgs::2DTargets"] = res_msg;
      _vis_pub.publish(json_image);
    }
  }
}


nlohmann::json ArucoDetCppNode::detect(cv::Mat img_)
{

  double t1 = sms::get_time_sec();
  // 获得参数服务器中的参数dictionaryId
  this->_dictionary_id = this->get_param("dictionaryId");
  if (this->_dictionary == nullptr)
  {
    this->_dictionary = new aruco::Dictionary;
    *(this->_dictionary) = aruco::getPredefinedDictionary(this->_dictionary_id);
    this->_last_dictionary_id = this->_dictionary_id;
  }
  else if (this->_last_dictionary_id != this->_dictionary_id)
  {
    *(this->_dictionary) = aruco::getPredefinedDictionary(this->_dictionary_id);
    this->_last_dictionary_id = this->_dictionary_id;
  }

  std::vector<int> ids, ids_final;
  std::vector<std::vector<cv::Point2f> > corners, corners_final, rejected;
  std::vector<cv::Vec3d> rvecs, tvecs;

  // detect markers and estimate pose
  aruco::detectMarkers(img_, this->_dictionary, corners, ids, _detector_params, rejected);

  // 检测特殊的
  this->_pad_type = this->get_param("padType");

  if (ids.size() > 0)
  {
    if (_ids_need[0] == -1)
    {
      if (this->_pad_type == ArucoPadType::FEIPad)
      {
        std::vector<cv::Vec3d> t_rvecs_s, t_rvecs_b, t_tvecs_s, t_tvecs_b;
        std::vector<std::vector<cv::Point2f> > t_corners_s, t_corners_b;
        for (int j=0; j<ids.size(); j++)
        {
          if (ids[j] >= 1 && ids[j] <= 57)
          {
            t_corners_s.push_back(corners[j]);
            ids_final.push_back(ids[j]);
            corners_final.push_back(corners[j]);
          }
        }
        for (int j=0; j<ids.size(); j++)
        {
          if (ids[j] >= 91 && ids[j] <= 94)
          {
            t_corners_b.push_back(corners[j]);
            ids_final.push_back(ids[j]);
            corners_final.push_back(corners[j]);
          }
        }
        aruco::estimatePoseSingleMarkers(t_corners_s, _lengths_need[0], this->camera_matrix, this->distortion, t_rvecs_s, t_tvecs_s);
        aruco::estimatePoseSingleMarkers(t_corners_b, _lengths_need[0] * 4, this->camera_matrix, this->distortion, t_rvecs_b, t_tvecs_b);
        for (auto t_rvec : t_rvecs_s)
            rvecs.push_back(t_rvec);
        for (auto t_rvec : t_rvecs_b)
            rvecs.push_back(t_rvec);
        for (auto t_tvec : t_tvecs_s)
            tvecs.push_back(t_tvec);
        for (auto t_tvec : t_tvecs_b)
            tvecs.push_back(t_tvec);
      }
      else
      {
        aruco::estimatePoseSingleMarkers(corners, _lengths_need[0], this->camera_matrix, this->distortion, rvecs, tvecs);
        ids_final = ids;
        corners_final = corners;
      }
    }
    else
    {
      for (int i=0; i<_ids_need.size(); i++)
      {
        int id_need = _ids_need[i];
        double length_need = _lengths_need[i];
        std::vector<cv::Vec3d> t_rvecs, t_tvecs;
        std::vector<std::vector<cv::Point2f> > t_corners;
        for (int j=0; j<ids.size(); j++)
        {
          if (ids[j] == id_need)
          {
            t_corners.push_back(corners[j]);
            ids_final.push_back(ids[j]);
            corners_final.push_back(corners[j]);
          }
        }
        if (t_corners.size() > 0)
        {
          aruco::estimatePoseSingleMarkers(t_corners, length_need, this->camera_matrix, this->distortion, t_rvecs, t_tvecs);
          for (auto t_rvec : t_rvecs)
            rvecs.push_back(t_rvec);
          for (auto t_tvec : t_tvecs)
            tvecs.push_back(t_tvec);
        }
      }
    }
  }

  nlohmann::json msg = this->_publish_to_sms(ids_final, corners_final, rvecs, tvecs, img_.cols, img_.rows);

  if (this->_pad_type == ArucoPadType::FEIPad)
  {
    this->_fei_pad_recognition(msg);
  }

  _res_pub.publish(msg);

  return msg;  
}


nlohmann::json ArucoDetCppNode::_publish_to_sms(
  std::vector<int>& ids_,
  std::vector<std::vector<cv::Point2f> >& corners_,
  std::vector<cv::Vec3d>& rvecs_,
  std::vector<cv::Vec3d>& tvecs_,
  int im_width,
  int im_height
)
{
  nlohmann::json msg = sms::def_msg("spirecv_msgs::2DTargets");
  msg["width"] = im_width;
  msg["height"] = im_height;
  msg["fov_x"] = this->fov_x;
  msg["fov_y"] = this->fov_y;
  if (this->_t0 > 0)
  {
    msg["fps"] = 1.0 / (sms::get_time_sec() - this->_t0 + 1e-6);
  }
  else
  {
    msg["fps"] = 0;
  }
  this->_t0 = sms::get_time_sec();

  msg["targets"] = nlohmann::json::array();
  for (int i=0; i<ids_.size(); i++)
  {
    std::vector<cv::Point2f> corner_ = corners_[i];
    nlohmann::json target;
    target["score"] = 1.0;
    target["category_name"] = "d" + std::to_string(_dictionary_id) + "-" + std::to_string(ids_[i]);
    target["category_id"] = ids_[i];
    target["instance_id"] = ids_[i];
    double left = std::min(std::min(corner_[0].x, corner_[1].x), std::min(corner_[2].x, corner_[3].x));
    double right = std::max(std::max(corner_[0].x, corner_[1].x), std::max(corner_[2].x, corner_[3].x));
    double top = std::min(std::min(corner_[0].y, corner_[1].y), std::min(corner_[2].y, corner_[3].y));
    double bottom = std::max(std::max(corner_[0].y, corner_[1].y), std::max(corner_[2].y, corner_[3].y));
    target["bbox"] = {left, top, right - left + 1, bottom - top + 1};

    double x_mid = (corner_[0].x + corner_[1].x) / 2.;
    double y_mid = (corner_[0].y + corner_[1].y) / 2.;
    double x_vec = x_mid - (left + right) / 2.;
    double y_vec = y_mid - (top + bottom) / 2.;
    target["yaw_a"] = this->_calc_yaw(x_vec, y_vec);

    double cx = (double)(right + left) / 2 / im_width;
    double cy = (double)(bottom + top) / 2 / im_height;
    double los_ax = atan((cx * im_width - im_width / 2.) / this->camera_matrix.at<double>(0, 0)) * SV2_RAD2DEG;
    double los_ay = atan((cy * im_height - im_height / 2.) / this->camera_matrix.at<double>(1, 1)) * SV2_RAD2DEG;
    target["los"] = {los_ax, los_ay};
    target["position"] = {tvecs_[i][0], tvecs_[i][1], tvecs_[i][2]};
    target["rvec"] = {rvecs_[i][0], rvecs_[i][1], rvecs_[i][2]};

    msg["targets"].push_back(target);
  }
  return msg;
}


double ArucoDetCppNode::_calc_yaw(double vec_x_, double vec_y_)
{
  double yaw_a;
  if (vec_x_ == 0. && vec_y_ > 0.)
  {
    yaw_a = 180;
  }
  else if (vec_x_ == 0. && vec_y_ < 0.)
  {
    yaw_a = 0;
  }
  else if (vec_x_ > 0. && vec_y_ == 0.)
  {
    yaw_a = 90;
  }
  else if (vec_x_ > 0. && vec_y_ > 0.)
  {
    yaw_a = 180 - atan(vec_x_ / vec_y_) * SV2_RAD2DEG;
  }
  else if (vec_x_ > 0. && vec_y_ < 0.)
  {
    yaw_a = atan(vec_x_ / -vec_y_) * SV2_RAD2DEG;
  }
  else if (vec_x_ < 0. && vec_y_ == 0.)
  {
    yaw_a = -90;
  }
  else if (vec_x_ < 0. && vec_y_ > 0.)
  {
    yaw_a = atan(-vec_x_ / vec_y_) * SV2_RAD2DEG - 180;
  }
  else if (vec_x_ < 0. && vec_y_ < 0.)
  {
    yaw_a = -atan(-vec_x_ / -vec_y_) * SV2_RAD2DEG;
  }
  return yaw_a;
}


// 以下代码为FEI降落靶标专用
bool ArucoDetCppNode::_fill_value_from_id(float id2c_t[3], int id, float unit_len, double x_b, double y_b, int id_begin)
{
    if (id == 1)
  {
    id2c_t[0] = -28 * unit_len;
    id2c_t[1] = -28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 2)
  {
    id2c_t[0] = -23 * unit_len;
    id2c_t[1] = -28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 3)
  {
    id2c_t[0] = -18 * unit_len;
    id2c_t[1] = -28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 4)
  {
    id2c_t[0] = -10 * unit_len;
    id2c_t[1] = -28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 5)
  {
    id2c_t[0] = -5 * unit_len;
    id2c_t[1] = -28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 6)
  {
    id2c_t[0] = 0 * unit_len;
    id2c_t[1] = -28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 7)
  {
    id2c_t[0] = 5 * unit_len;
    id2c_t[1] = -28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 8)
  {
    id2c_t[0] = 10 * unit_len;
    id2c_t[1] = -28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 9)
  {
    id2c_t[0] = 18 * unit_len;
    id2c_t[1] = -28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 10)
  {
    id2c_t[0] = 23 * unit_len;
    id2c_t[1] = -28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 11)
  {
    id2c_t[0] = 28 * unit_len;
    id2c_t[1] = -28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 12)
  {
    id2c_t[0] = 28 * unit_len;
    id2c_t[1] = -23 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 13)
  {
    id2c_t[0] = 28 * unit_len;
    id2c_t[1] = -18 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 14)
  {
    id2c_t[0] = 28 * unit_len;
    id2c_t[1] = -10 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 15)
  {
    id2c_t[0] = 28 * unit_len;
    id2c_t[1] = -5 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 16)
  {
    id2c_t[0] = 28 * unit_len;
    id2c_t[1] = 0 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 17)
  {
    id2c_t[0] = 28 * unit_len;
    id2c_t[1] = 5 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 18)
  {
    id2c_t[0] = 28 * unit_len;
    id2c_t[1] = 10 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 19)
  {
    id2c_t[0] = 28 * unit_len;
    id2c_t[1] = 18 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 20)
  {
    id2c_t[0] = 28 * unit_len;
    id2c_t[1] = 23 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 21)
  {
    id2c_t[0] = 28 * unit_len;
    id2c_t[1] = 28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 22)
  {
    id2c_t[0] = 23 * unit_len;
    id2c_t[1] = 28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 23)
  {
    id2c_t[0] = 18 * unit_len;
    id2c_t[1] = 28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 24)
  {
    id2c_t[0] = 10 * unit_len;
    id2c_t[1] = 28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 25)
  {
    id2c_t[0] = 5 * unit_len;
    id2c_t[1] = 28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 26)
  {
    id2c_t[0] = 0 * unit_len;
    id2c_t[1] = 28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 27)
  {
    id2c_t[0] = -5 * unit_len;
    id2c_t[1] = 28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 28)
  {
    id2c_t[0] = -10 * unit_len;
    id2c_t[1] = 28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 29)
  {
    id2c_t[0] = -18 * unit_len;
    id2c_t[1] = 28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 30)
  {
    id2c_t[0] = -23 * unit_len;
    id2c_t[1] = 28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 31)
  {
    id2c_t[0] = -28 * unit_len;
    id2c_t[1] = 28 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 32)
  {
    id2c_t[0] = -28 * unit_len;
    id2c_t[1] = 23 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 33)
  {
    id2c_t[0] = -28 * unit_len;
    id2c_t[1] = 18 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 34)
  {
    id2c_t[0] = -28 * unit_len;
    id2c_t[1] = 10 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 35)
  {
    id2c_t[0] = -28 * unit_len;
    id2c_t[1] = 5 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 36)
  {
    id2c_t[0] = -28 * unit_len;
    id2c_t[1] = 0 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 37)
  {
    id2c_t[0] = -28 * unit_len;
    id2c_t[1] = -5 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 38)
  {
    id2c_t[0] = -28 * unit_len;
    id2c_t[1] = -10 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 39)
  {
    id2c_t[0] = -28 * unit_len;
    id2c_t[1] = -18 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 40)
  {
    id2c_t[0] = -28 * unit_len;
    id2c_t[1] = -23 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 41)
  {
    id2c_t[0] = 0.;
    id2c_t[1] = 0.;
    id2c_t[2] = 0.;
  }
  else if (id == 42)
  {
    id2c_t[0] = 0.;
    id2c_t[1] = -5 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 43)
  {
    id2c_t[0] = 5 * unit_len;
    id2c_t[1] = 0.;
    id2c_t[2] = 0.;
  }
  else if (id == 44)
  {
    id2c_t[0] = 0.;
    id2c_t[1] = 5 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 45)
  {
    id2c_t[0] = -5 * unit_len;
    id2c_t[1] = 0.;
    id2c_t[2] = 0.;
  }
  else if (id == 46)
  {
    id2c_t[0] = 0.;
    id2c_t[1] = -10 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 47)
  {
    id2c_t[0] = 10 * unit_len;
    id2c_t[1] = 0.;
    id2c_t[2] = 0.;
  }
  else if (id == 48)
  {
    id2c_t[0] = 0.;
    id2c_t[1] = 10 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 49)
  {
    id2c_t[0] = -10 * unit_len;
    id2c_t[1] = 0.;
    id2c_t[2] = 0.;
  }

  else if (id == 50)
  {
    id2c_t[0] = 0.;
    id2c_t[1] = -23 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 51)
  {
    id2c_t[0] = 23 * unit_len;
    id2c_t[1] = 0.;
    id2c_t[2] = 0.;
  }
  else if (id == 52)
  {
    id2c_t[0] = 0.;
    id2c_t[1] = 23 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 53)
  {
    id2c_t[0] = -23 * unit_len;
    id2c_t[1] = 0.;
    id2c_t[2] = 0.;
  }
  else if (id == 54)
  {
    id2c_t[0] = 0.;
    id2c_t[1] = -18 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 55)
  {
    id2c_t[0] = 18 * unit_len;
    id2c_t[1] = 0.;
    id2c_t[2] = 0.;
  }
  else if (id == 56)
  {
    id2c_t[0] = 0.;
    id2c_t[1] = 18 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 57)
  {
    id2c_t[0] = -18 * unit_len;
    id2c_t[1] = 0.;
    id2c_t[2] = 0.;
  }

  else if (id == 91)
  {
    id2c_t[0] = -14 * unit_len;
    id2c_t[1] = -14 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 92)
  {
    id2c_t[0] = 14 * unit_len;
    id2c_t[1] = -14 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 93)
  {
    id2c_t[0] = -14 * unit_len;
    id2c_t[1] = 14 * unit_len;
    id2c_t[2] = 0.;
  }
  else if (id == 94)
  {
    id2c_t[0] = 14 * unit_len;
    id2c_t[1] = 14 * unit_len;
    id2c_t[2] = 0.;
  }

  else
  {
    return false;
  }
  
  id2c_t[0] -= x_b;
  id2c_t[1] -= y_b;

  id2c_t[0] = -id2c_t[0];
  //id2c_t[1] = 0. - id2c_t[1];
  //id2c_t[2] = 0. - id2c_t[2];
  return true;
}


void ArucoDetCppNode::_fei_pad_recognition(nlohmann::json& msg)
{
  // logger.debug("ArucoPadType::FEIPad");

  std::vector<float> cpx, cpy, cpz, cyaw;
  // std::vector<float> wpx, wpy, wpz;
  std::vector<bool> is_bigids;

  for (int i=0; i<msg["targets"].size(); i++)
  {
    cv::Mat rotation_matrix;
    cv::Vec3d rvec_(msg["targets"][i]["rvec"][0], msg["targets"][i]["rvec"][1], msg["targets"][i]["rvec"][2]);
    cv::Rodrigues(rvec_, rotation_matrix);

    float id2c_t[3];
    if (_fill_value_from_id(id2c_t, msg["targets"][i]["category_id"], _lengths_need[0] / 4., this->_fei_x_bias, this->_fei_y_bias, this->_fei_id_begin))
    {
      cv::Mat id2c_t_mat = cv::Mat(3, 1, CV_32FC1, id2c_t);
      rotation_matrix.convertTo(rotation_matrix, CV_32FC1);
      // cv::invert(rotation_matrix, rotation_matrix);

      std::vector<double> vec_t{msg["targets"][i]["position"][0], msg["targets"][i]["position"][1], msg["targets"][i]["position"][2]};
      cv::Mat vec_t_mat{vec_t};
      vec_t_mat.convertTo(vec_t_mat, CV_32FC1);
      // 计算标定板子原点，在当前二维码下到坐标: rotation_matrix * id2c_t_mat
      // 机体坐标系下，到标定版中点的坐标: cent_t
      cv::Mat cent_t = rotation_matrix * id2c_t_mat + vec_t_mat;
      
      // cv::Mat cam_in_tgt = -rotation_matrix.t() * cent_t;
      // std::cout << cam_in_tgt << std::endl;
      // wpx.push_back(cam_in_tgt.at<float>(0));
      // wpy.push_back(cam_in_tgt.at<float>(1));
      // wpz.push_back(cam_in_tgt.at<float>(2));

      cpx.push_back(cent_t.at<float>(0));
      cpy.push_back(cent_t.at<float>(1));
      cpz.push_back(cent_t.at<float>(2));
      cyaw.push_back(msg["targets"][i]["yaw_a"]);
      if (msg["targets"][i]["category_id"] >= 91 && msg["targets"][i]["category_id"] <= 94)
        is_bigids.push_back(true);
      else
        is_bigids.push_back(false);
    }
  }

  int n_total = (int)cpx.size();
  int n_selected = (int)ceil(cpx.size() * 0.7);
  int n_try = 0;
  float ransac_error = 1e5;
  int n_force = 0;
  float meanpx, meanpy, meanpz, meanyaw;
  float w_meanpx, w_meanpy, w_meanpz;
  while (ransac_error > 0.02 && n_selected > 0 && n_try < 100)
  {
    n_try += 1;
    std::vector<int> selected_ids;
    std::vector<int> force_ids;
    for (int i=0; i < n_total; i++)
    {
      selected_ids.push_back(i);
      if (is_bigids[i])
        force_ids.push_back(i);
    }
    int n_force_size = (int)force_ids.size();
    random_shuffle(selected_ids.begin(), selected_ids.end());
    n_force = std::min(n_selected, n_force_size);
    for (int i=0; i < n_force; i++)
    {
      selected_ids[i] = force_ids[i];
    }
    float meanpx1(0), meanpy1(0), meanpz1(0), meanyaw1(0);
    // float w_meanpx1(0), w_meanpy1(0), w_meanpz1(0);
  
    for (int i=0; i < n_selected; i++)
    {
      meanpx1 += cpx[selected_ids[i]];
      meanpy1 += cpy[selected_ids[i]];
      meanpz1 += cpz[selected_ids[i]];

      // w_meanpx1 += wpx[selected_ids[i]];
      // w_meanpy1 += wpy[selected_ids[i]];
      // w_meanpz1 += wpz[selected_ids[i]];

      meanyaw1 += cyaw[selected_ids[i]];
    }
    meanpx1 /= n_selected;
    meanpy1 /= n_selected;
    meanpz1 /= n_selected;
    meanyaw1 /= n_selected;

    // w_meanpx1 /= n_selected;
    // w_meanpy1 /= n_selected;
    // w_meanpz1 /= n_selected;

    float meanpx2(0), meanpy2(0), meanpz2(0), meanyaw2(0);
    // float w_meanpx2(0), w_meanpy2(0), w_meanpz2(0);
    selected_ids.clear();
    for (int i=0; i < n_total; i++)
    {
      selected_ids.push_back(i);
    }
    random_shuffle(selected_ids.begin(), selected_ids.end());
    for (int i=0; i < n_force; i++)
    {
      selected_ids[i] = force_ids[i];
    }
    for (int i=0; i < n_selected; i++)
    {
      meanpx2 += cpx[selected_ids[i]];
      meanpy2 += cpy[selected_ids[i]];
      meanpz2 += cpz[selected_ids[i]];

      // w_meanpx2 += wpx[selected_ids[i]];
      // w_meanpy2 += wpy[selected_ids[i]];
      // w_meanpz2 += wpz[selected_ids[i]];

      meanyaw2 += cyaw[selected_ids[i]];
    }
    meanpx2 /= n_selected;
    meanpy2 /= n_selected;
    meanpz2 /= n_selected;
    meanyaw2 /= n_selected;

    // w_meanpx2 /= n_selected;
    // w_meanpy2 /= n_selected;
    // w_meanpz2 /= n_selected;

    ransac_error = sqrt((meanpx2-meanpx1)*(meanpx2-meanpx1) + (meanpy2-meanpy1)*(meanpy2-meanpy1) + (meanpz2-meanpz1)*(meanpz2-meanpz1));
    // printf("  Aruco NUM = %d, SELETED = %d, ERR = %.3f\n", n_total, n_selected, ransac_error);
    meanpx = (meanpx2 + meanpx1) / 2;
    meanpy = (meanpy2 + meanpy1) / 2;
    meanpz = (meanpz2 + meanpz1) / 2;

    // w_meanpx = (w_meanpx2 + w_meanpx1) / 2;
    // w_meanpy = (w_meanpy2 + w_meanpy1) / 2;
    // w_meanpz = (w_meanpz2 + w_meanpz1) / 2;

    meanyaw = (meanyaw1 + meanyaw2) / 2;
  }
  if (n_force > 0 || n_selected > 1)
  {
    msg["fei_n_bigs"] = n_force;
    msg["fei_n_totals"] = n_total;
    msg["fei_n_selects"] = n_selected;
    msg["fei_ransac_error"] = ransac_error;
    msg["fei_yaw_a"] = meanyaw;
    msg["fei_position"] = {meanpx, meanpy, meanpz};
    // msg["fei_position_tf"] = {w_meanpx, w_meanpy, w_meanpz};

    double cx = camera_matrix.at<double>(0, 0) * meanpx / meanpz + camera_matrix.at<double>(0, 2);
    double cy = camera_matrix.at<double>(1, 1) * meanpy / meanpz + camera_matrix.at<double>(1, 2);
    if (cx >= 0 && cy >= 0 && !std::isnan(cx) && !std::isnan(cx))
      msg["fei_cxcy"] = {cx, cy};
  }
  else
  {
    msg["fei_n_selects"] = 0;
  }
  

}



}
